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ABSTRACT 


Feedback control of a two wheeled mobile robot from one point in its 
configuration space to another presents a challenging problem. The mobile robot 
belongs to a class of systems with non-integrable motion constraints for which 
smooth feedback control laws cannot be designed. Recent work has been aimed 
at developing time-varying feedback control laws and piecewise smooth feedback 
control laws. These control techniques are, however, not optimal in any sense. 
In this research, we look into the optimal control of a mobile robot using partial 
feedback. A solution is obtained by application of Pontryagin’s Minimization 
Priciple and solving the associated two point boundary value problem using a 
numerical relaxation technique. The resulting robot trajectories exhibit optimal 


behavior for all non-trivial cases. 
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I. 


INTRODUCTION 


The mobile robot belongs to a class of systems with non- 
integrable motion constraints for which smooth feedback 
control laws for motion from one point in the configuration 
space to another cannot be designed [Ref. 1]. Recent work 
aims at developing time-varying feedback control laws [Ref. 1] 
and piecewise smooth feedback control laws [Ref. 2] . These 
control techniques are, however, not optimal in any sense. In 
this research, we look into the optimal control of a mobile 
robot using partial feedback. 

A. KINEMATICS OF A MOBILE ROBOT 

The position and orientation of a two wheeled mobile robot 
on a horizontal plane is described by three generalized 
coordinates. Figure 1 shows the three coordinates chosen for 
our robot. These are the two X-Y coordinates for the location 
of the robot on the plane, and an angular displacement, 0, to 
describe the robot's orientation with respect to the positive 
X axis. 

The velocity of the robot can be described completely in 
terms of translation and rotation. Assuming no slipping, the 
interaction of the wheels with the plane restricts the 
instantaneous motion to the direction of orientation of the 
robot. Defining as the velocity in the direction of 



orientation, and Uj as the rate of change of the orientation, 
the following constraint equations result: 

X = COS0 

y = Oi sine (2) 



Note that while the constraints above limit the number of 
degrees of freedom for the system to two, specifically and 
Uj, a minimum of three coordinates are required to describe 
the system. This is true of all nonholonomic systems; the 
number of generalized coordinates required to describe the 
system is greater than the number of degrees of freedom. 

A nonholonomic system is characterized by the non- 
integrable nature of the constraint between the first 
derivatives of the coordinates. [Ref. 4:p. 244] In the 
particular case of the mobile robot, the non-integrable 
constraint is due to the nature of the angular displacement 
term, 9. As 0 is an independent function of time, the 
relationship between the remaining coordinates cannot be 
uniquely determined. In other words, for a robot moving from 
one position and orientation on the X-Y plane to another, the 
instantaneous value of 6 depends upon the path followed by the 
robot. As a result, the coordinate relationship is dependent 
upon the path taken. 
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OPTIMAL CONTROL 


B. 

Since the number of paths the robot could follow is 
infinite, some paths would be more efficient than others. In 
order to determine the most efficient path, we must first 
chose a cost function or a performance index. Following the 
development in Reference 5, pp. 180-183, for optimal control 
of a standard nonlinear system, we may obtain the necessary 
conditions for optimality. 

We first express the differential equations of motion in 
the form, 

x = f(x, u,t) (4) 

The cost function can take the form 

J- = <&[x(tf) , t^] *jF[x{t) ,u{t) ,t]dt (5) 

where F could represent the pseudo-kinetic energy in the form 
u^, with u as the velocity. The term $ is a terminal cost 
vector and is a function of the states at the final time. 
This final time is not specified. Applying Lagrange 
multiplier vector, \, we form the augmented functional. 

j=$+J[F+X^(f-x)] dc (6) 

After defining the Hamiltonian as 

(7) 

we can determine the necessary conditions for an optimal 
solution using Pontryagin's Minimization Principle: 

[Ref. 5:p. 183] 



(8) 



(9) 

(10) 

( 11 ) 

(12) 


"The optimal control u(t) is determined at each instant to 
render the Hamiltonian a minimum over all admissible control 
functions." [Ref. 5:p. 183] Using the last condition, we can 
solve for the control input, u, in terms of the states, x, and 
what we will now refer to as costates, X. 

Let us now consider some simplifications to the above 
necessary conditions. If we fix the final time to achieve the 
desired condition, the first criterion is immediately 
satisfied as = 0. If we also describe the desired 
condition directly in terms of the states, x, and fix the 
value of the desired final states, then 5x(tf) = 0. 
Consequently, the second condition is met. In practical 
terms, this translates to going to a desired set of states in 
a fixed amount of time. 

We now apply these simplifications to the differential 
expressions for the states and costates. Assuming our initial 
states are known, we have boundary conditions for the states 



at the initial and final time. However, we know neither the 
initial or final boundary conditions for the costates. And 
since the state and costate differential expressions are 
coupled, they must be solved simultaneously. As a result, the 
combined expressions give the form for a two point boundary 
value problem. 


C. TWO POINT BOUNDARY VALUE PROBLEMS 

In the case of linear differential equations, many 
analytic methods are available for solution of two point 
boundary value problems. However for nonlinear problems like 
the mobile robot, analytic methods for the solution to the two 
point boundary value problem do no exist. In some cases, non¬ 
linear problems can be solved analytically. Such problems are 
generally very simple and may only represent special cases of 
an overall problem. As we shall see later, the mobile robot 
problem does not lend itself readily to analytic methods. 

In many cases, a non-linear two point boundary value 
problem can best be solved numerically. Unfortunately, 
numerical methods for non-linear two point boundary value 
problems are usually fairly complicated. 



The general approach is to make an initial "guess" to the 
solution and adjust this trial solution to match the boundary 
conditions and differential equations. There are two distinct 
methods for solving such problems, shooting and relaxation 
[Ref. 6], 

The first method, shooting, requires an initial guess of 
dependent variables based upon one boundary. Then using 
numerical methods common to initial value problems, we obtain 
a trial solution. This trial solution is compared against the 
second boundary. The error between the two is noted and the 
free parameters of the equation adjusted accordingly. This 
repeats until the error is sufficiently small. The advantages 
of this method are its simplicity and relative speed. For 
extremely non-linear systems, however, systematically 
improving the solution can prove difficult. 

In the second method, relaxation, the differential 
equations are converted into difference expressions using 
Taylor series expansion. With an arbitrary initial trial 
solution, the variance of each point in the discretized mesh 
is calculated. The trial solution is then adjusted to improve 
agreement with the differential equations and the boundary 
conditions. This continues iteratively until the variance, or 
error, of the solution is sufficiently small. Relaxation 
methods are considered advantageous for problems with 
complicated boundary conditions, but smooth and non- 
oscillatory functions. Two disadvantages of this method are 



the large number of variables to be solved simultaneously and 
complexity of the expressions required in the algorithm. The 
number of differential equations, mesh size and coupling of 
adjacent points in the mesh determine the number of variables 
to solve. For example, in a system with 8 differential 
equations, on a mesh with 100 points, coupling two points, 
1600 variables would result. 

For the mobile robot problem, the kinematic equations 
involve trigonometric functions. As we shall see in Chapter 
III, the resulting state and costate differential equations 
are highly non-linear. In anticipation of the highly non¬ 
linear kinematic behavior of a mobile robot, the approach 
taken here is the relaxation method. We take advantage of 
published computer programs designed specifically for this 
method. 

D. APPLICATION OF THE RELAXATION METHOD 

As previously stated, the method starts with an initial 
guess trajectory for each of the differential equations and 
then adjusts these trial solutions to match both the governing 
equations and the boundary conditions. The method in which 
the computer program makes the corrections to the trajectories 
is a key to finding a proper solution. The source of the 
computer code and expression preparation process used here is 


Reference 6. 



Given a set of N coupled first order differential 
equations, we first divide the independent variable domain 
into M discrete mesh points, tjj., k= 1,2, . .M. For our problem, 
the initial state boundary values are located at tj and the 
final state boundary values at The costate boundary 

values are not fixed. The N differential equations then 
become finite difference equations to describe the internal 
meshpoints. We define the vector as the entire set of 
dependent variables at point The exact form of the finite 

difference equation depends on the coupling desired. For our 
purposes, a backward difference technique is sufficient. This 
will couple each point on the mesh with the point preceding 
it. 

By comparing the difference between adjacent solution 
values, (Yk-Yk-i^ > to the solution of the finite difference 
equations, we form an error equation. A solution exists where 
the error equations are zero and the boundary conditions are 
met. Considering any internal mesh point, k, this error 
expression takes the form 

Through Taylor series expansion of the error equation we 
determine the variance of the error with small changes in 
Since we are looking for the solution where the error is zero, 
for the internal mesh points, k=2,3...M, the form is 



j=l,2 ,...N 


dE. 


dE. 




(15) 


(16) 


At each internal point, k, forms a. N X 2N matrix. The 
contents of this matrix are corrections to the solution 
variables between points k and k-1. 

At the initial boundary, since depends only on the 
relation takes the form 


where 


„Ay„ , = 

j=n2+l.n2+2, . . . ,N 


(17) 


dE. , 


(18) 


n=l,2 ,....N 

And similarly, at the final boundary, where depends 
only on ynj, the form is 


where 


J=l,2, . 


(19) 




3y„,„ ' 


( 20 ) 


n=l,2 ,...,N 





The above equations can now be used to solve for 
corrections, Ay, to the trial solution vector, y. This 
process continues iteratively until the correction are 
sufficiently small. Of course, since the equations are 
coupled, they must be solved simultaneously. 

If we combine the expressions for each internal point and 
boundary points in a global matrix, we see that matrix has a 
special "block diagonal" form (Fig. 2 ). This form allows a 
more economical matrix inversion process. The matrix 
inversion is accomplished through a form of Gaussian 
elimination which takes advantage of the special form. This 
process requires significantly less computational time or 
storage than inversion of the entire matrix. This is critical 
due to the size of the global matrix, (MN X MN) . 

Recall that our overall goal is to determine the optimal 
trajectory for a mobile robot traversing from one position and 
orientation to another. Application of optimal control theory 
results in a two point boundary value problem. Using the 
method described above, we can solve most problems of this 
form. However, this method does not guarantee a solution. 
Many factors will affect the program’s ability to converge to 
a solution. Therefore before attempting the two wheeled 
mobile robot problem, a simpler related problem will be 
solved. This will serve to provide insight on use of the 
program and validate the process. 



II. OPTIMAL CONTROL OF A ROLLING DISK 


In Chapter I, we provided an outline for the optimal 
control problem of a dynamical system. In this chapter we 
apply Pontryagin's Minimization Principle [Ref. 5] and solve 
the associated two point boundary value problem for the simple 
example of a rolling disk. The differential equations of 
motion for the disk and robot systems are similar, and the 
nonholonomic constraint is exactly the same; no side slipping 
is allowed. The only difference between the rolling disk and 
the mobile robot model is the addition of a state variable: 
the angular orientation of the rolling disk about it's 
rotational axis, 0. 

A. PROBLEM DESCRIPTION 

Consider a vertical disk rolling on the horizontal, X-Y 
plane. (Fig. 3). Like the mobile robot, the orientation of 
this disk with respect to the plane will be described as an 
angular displacement, B, from the X axis. The orientation of 
the disk face with respect to it's axis of rotation is 
described as an angular displacement, (#>, from the normal 
vector to the X-Y plane. This gives us a total of 5 
coordinates to describe the position and orientation of the 
disk. 
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The velocity of the disk, like the robot, can be described, 
in terms of translation and rotation. The translational 
velocity again is constrained to the direction of orientation 
of the disk. However, the forward velocity of the disk is 
directly related to the angular velocity of 4> and disk radius, 
R. If we consider the variation of 8 and <}> with time as 
control inputs, and U 2 respectively, the state-space form 
of the kinematic equations becomes 

0 R cos6 
0 R sin0 

1 0 H 
.0 1 

or in a more condensed form as 

i = [if] u 


\x 

\y 

le 

w 


(21) 


(22) 


B. OPTIMAL CONTROL 

The objective for this problem is to roll the disk from 
and initial position and orientation to a desired final 
position and orientation in some optimal manner. Note that 
for our problem the time to accomplish this task is fixed. 
The choice of units for the X-Y parameters are arbitrary. The 
angular displacements are in non-dimensional radians. Time is 
considered on the unity scale with 0 at tg to 1 at tf. The 
initial conditions at tg are defined as 0^, 4>i, and the 
final conditions at as Xg, Y^, 8^, 
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The development of the optimal control problem follows the 
method described in Chapter I. To determine an optimal path 
for the disk, we define the performance parameter as 


J = 


(23) 


Since the terminal costs are path independent, they are 
neglected here. Adding a Lagrange multiplier the cost 
functional becomes 


J = f (ju^u X'^(Ku - x))dC (24) 

By defining the Hamiltonian, 

H = ^{u'^u + X'^Ku) (25) 

the optimal control is obtained as: 

u = -K-^X (26) 

Substituting this expression into equation (25), the 
Hamiltonian becomes 

H = -| (X^ K K'^ X) (27) 

Using this new expression, the states can be expressed as 

X = -K K'^ X (28) 

or in expanded form. 
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- (i?cos0A,j^ + iJsinSXj + 

Similarly, the costates equations can be expressed as 

i = 

Noting that the matrix K is only a function of state variable 
6, the individual costate equations become 


= 0 
I'j = 0 

^ - xl) (sin2e + 2 + cos 20 ) ] 

+J?Xj(X 2 Cos 0 - X^sinO) 

X4 = 0 

Combining the states and costates into a single vector gives 
the structure for the two point boundary value problem. 
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X 

Y 

0 

(t> 

X, 

^2 

^2 

X, 


_-l (1 + COS26) - sin2 0 - i^cosOX^j 

|- ^2^ sin 20 - ^ (1 - COS20) - i?sin0A^j 


[-^3] 

[-(i?cos0Ai + i?sin0A2 
0 
0 


A.)] 


(32) 


-:|^[(A^-X?)sin2e + 2 A 2 A 2 + cos20]+ 
rX^ (XjCosO-Aj^sinO) 

0 


To the best of our knowledge, no analytical solution exists to 
this problem. A similar problem has been solved analytically 
by Cameron [Ref. 7] . However, his problem looks for the 
minimum time solution. By use of Pontryagin's Minimization 
Principle, equation (12), this implies use of the time 
derivative of the Hamiltonian. For the minimum time problem, 
it can be shown that the Hamiltonian is a constant. However 
in our problem, the final time is fixed and terminal cost, <l>, 
is zero. From equation (8), the Hamiltonian may therefore be 
any value over time. Therefore, Cameron's analytical method 
does not apply to our fixed time problem. 
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C. HOMERICAL SOLUTION BY THE RELAXATION METHOD 


Given the N differential equations above, we apply the 
relaxation method described in Chapter I to develop 
expressions required by the relaxation method computer 
program. This essential entails finding the elements of the 
S matrix. For the interior meshpoints, a total of N X 2N such 
expressions must be developed. The two boundaries each 
require an additional N X N expressions. Since there are 
eight differential equations, we must develop a total of 144 
Sj expressions. Fortunately, many of the expressions for 
this particular problem will turn out to be zero. 

Rather than repeating the development for all these 
expressions here, an exanple of developing expressions for an 
interior point is presented. Given a differential equation 
which describes the interior mesh points, the first step is to 
substitute for all dependent variables, where n is the 
equation number, such that 

e-y. 


Ai-^s 

i3-y. 


We then apply the finite difference expression 



(34) 
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to each independent variable. Taking the first state equation 
as an example, the finite difference equation is 


2 2 \ 

2 2 
r, + 




sin2 (- 

cos 


Next, the finite difference equation is placed into the error 
expression. 


^l.k = -A 


-R^ (ys.ir ^ y5.;c-i)[ 


-A! Li 

2 2V 


cos ( - 


Where h is the grid spacing on the mesh. For our evenly- 
spaced mesh. 


h 


(37) 


As given by equation (16) , the Sj,n expressions are the 
partial derivatives of the error expressions with respect to 
each of the states and costates at meshpoint k and k-1. 
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Again, in the interest of brevity, only two 
expressions are presented here. Taking Sj 5 as the first 
example yields 

dE, 1 . h 

S - - )) (38) 

Fortunately, due to the finite difference method chosen, these 
expressions tend to repeat. For example 23 yields 
dE, L. hr?2 

13 = -5-^ = ^^(l + Cos(y3 (39) 

' °^5.k ^ 

the same as 5 . The development of the other 126 interior 
meshpoint expressions follow similarly, some simpler than 

others. The final result for all of these terms can be seen 
in the DIFEQ.FOR subroutine in Appendix A. 

The expressions for the boundary expressions, though 

similar, fall under equations (18) and (20) . The major 
difference for the boundary expressions is that they are not 
based on the differential equations. Since our boundary 
conditions are simply state values, the error expressions are 
at the initial and final time are of the form 



where n is the nth variable as given by equation (33). Thus 
for the initial and final boundary conditions respectively, 
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1 , for j=n 
0 , for j*n 



(j = l,2,- N) 

{n=l,2, ... ,m 


(41) 


where N is the total number equations and is the number of 
boundary conditions at the initial time. The shift in indices 
by N and is necessary to take advantage of the 'block 
diagonal' form of the overall matrix of S expressions. The 
result is the unity matrix for the initial and final S 
expressions. Note that for more complicated boundary 
conditions, such as a manifold of states or terminal costs, 
the relationships above are not valid. 

Next, we must develop an initial guess for the values of 
the states and costates for all points on the mesh. For the 
states this guess can be somewhat intuitive. For example, we 
desire that the disk start at the X,Y position (0,0) and roll 
ten (10) times and make one (1) complete turn to return to the 
starting position. Therefore, the initial and final boundary 
conditions are 


= 0 = 0 



where the angular 
Intuitively, we 


terms are expressed in radians, 
expect the most optimal path in the X-Y 


would 
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plane to be a circle. If we initially assume that the angular 
terms vary at a constant rate, the initial guess trajectory 
for the state variables will appear as shown in Figure 4. 
Since we have no information on the costate behavior, we will 
assume the initial trajectory for each costate to be a 
constant value of zero. 

After the required expressions and initial guess entry 
method is successfully compiled, the program is ready to run. 
A sampling of the results follow. 


D. DISCUSSION OF RESULTS 

For the case described above, the program converges in a 
few hundred iterations. From Figure 5, we see that the final 
state solution is in fact the same as the initial guess. The 
iterations were required to adjust the costate solutions to 
their proper trajectories. (Fig. 6) Since the state solution 
gives the expected circular path, the solution appears to be 
optimal. 

For a more rigorous validation, we substitute the costate 
solutions 


X^{t )=0 
x^{t) =0 
XjCt) =-2n 

( t) =-20ii 

back into equation (32) . 


(43) 
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The derivative equations can then be expressed a 


X = 2Oitcos0 
Y = 2Oitsin0 
0 = 2ir 
^ = 2011 

(44) 

= 0 
= 0 
Xj = 0 
= 0 

Note that the angular velocity terms are constant. This is 
consistent with the minimization of our cost function. And 
since this is a kinematics problem, the velocities may be non¬ 
zero at the initial and final time. Integrating the state 
terms yields 

Xit) =lOsin0 

Y(t)=10(1-COS0) , 4=5 

0(t)=2itt 

4 > (t) = 20 it t 

which gives the equation for a circle in the X-Y plane. 

If we make a slightly different initial guess for the 
states, such as an ellipse (Fig. 7) the final result is the 
same. If however, the initial guess is not sufficiently good, 
the program does not converge. While the initial guess for 
the states can usually be based on some intuitive reasoning, 
providing a sufficiently good estimate of the costate can 
prove difficult. For this problem, an initial guess of all 
zeros for costates works quite well. If, however, we chose 
trial values that are 10 units away from the proper solution, 
21 



the program does not converge. Thus, while the costates may 
not be particularly important to the usable state space 
solution, they are necessary to solve the optimal control 
problem. Generally though, a poor estimation of the costate 
can be compensated for by a good state estimation. 

Where the circular path presents a fairly simple solution, 
we now choose a more difficult task for our disk. This will 
demonstrate the usefulness of this method for problems where 
the optimal path is not obvious. For example, we desire that 
the disk make 10 rolls and 5.5 turns while moving on the X-Y 
axis form a point (10,10) to a point (-5,-2) . As an initial 
guess, we shall use the state and costate solution to the 
circular problem above. The resulting path, obtained after 
several hundred iterations, appears in Figure 8. The state 
and costate trajectories appear in Figures 9 and 10, 
respectively. While these solutions appear optimal, they are 
not obvious at the outset of the problem. 

The program for the disk problem has been tested 
extensively and, when provided a sufficiently good guess, 
found to give an apparently optimal solution for all cases 
except one. For the case of rolling the disk where the 
initial and final 6 boundary conditions are the saune and lie 
along the same line, the program does not converge. However, 
the program will converge if there is at least a very small 
difference between the initial and final angles. For the 
nearly straight line case, the smallest angular difference 
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which results in a convergence is .00036 degrees, (Fig. 11) 
To achieve this it is necessary reduce the SLOWC program 
parameter to cause smaller adjustments to the trial solution. 
This indicates that for small difference in boundary 0 values, 
the program is sensitive to small changes. If we exaggerate 
the distance the disk must roll between these two points, the 
reason for this behavior becomes evident. (Fig. 12) Here we 
specified that the disk roll 5 times. The initial 0 value is 
45 degrees. If we require that the disk make a 3.6 x 10'® 
degree turn to the left (1 X 10'^® rotation) we obtain one 
optimal solution. However, if we require that the disk make 
a 3.6 X 10'® degree turn to the right (-1 X 10'^® rotation) we 
obtain a much different solution. Hence, for very small 
changes in angle the solution varies widely. If we specify a 
zero rotation, there is no clear preference for the most 
optimal solution, and the program cannot converge. The same 
holds true as we approach a perfectly straight line path. We 
specify the initial and final position and the initial and 
final (f) values, which theoretically are the same. However 
numerically, there is a small difference. This difference is 
sufficient to induce the problems above and prevent 
convergence. Fortunately, an analytic solution to the exact 
straight line problem is easily obtained. 
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The computer program used includes several control 
parameters which assist in finding a solution. While running 
various simulations, the following trends were noted: 

• Slowing down the convergence by decreasing SLOWC can 
help find a solution when the maximum error fluctuates near 
some minimum value. However, doing so does not guarantee a 
solution. 

• The SCALV values should represent the absolute magnitude 
of a typical solution value. Where this value is not known, 
use a small SCALV to start. 

• The trend of the maximum error with iterations should be 
used as a guide as to whether the program will converge to a 
solution. However, the trend neither guarantees nor excludes 
convergence. 

As demonstrated, the rolling disk problem requires a path 
which is continuous and smooth. And since we specify the 
distance the disk must roll and number of turns the disk must 
make, the solution is only optimal for those specifications. 
In the more general mobile robot problem, we look for the most 
optimal path which need only meet the initial and final 
boundary conditions. 
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III. OPTIMAL CONTROL OF A MOBILE ROBOT 


In Chapter II we developed a two point boundary value 
problem by applying Pontryagin’s Minimization Principle to the 
equations of motion for a rolling disk. We then solved the 
resulting two point boundary value problem by a numerical 
relaxation technique. Having demonstrated that the process 
above provides an optimal solution for a simple nonholonomic 
system, we return to the more difficult mobile robot problem. 
Our goal is to move the robot from a initial position and 
orientation to a desired one within a fixed amount of time, in 
an optimal manner, and using feedback control. Before 
developing our solution, however, we first look at some 
conventional theory regarding mobile robot control. This is 
necessary to motivate the approach used to solve our problem. 

A. LITERATURE SURVEY 

Extensive research into non-linear control design of two 
wheeled mobile robots exists. For the problems of path 
following and tracking, relatively classical non-linear 
control techniques have been applied successfully. [Refs. 2,3] 
However, the problem of stabilization about a point is more 
difficult. Brockett's Theorem [Ref. 8] shows that smooth non¬ 
time varying control laws cannot be developed for such 
problems. 
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This is the case for all driftless, nonholonomic systems of 



Using classical Lyapunov analysis, Reference 1 presents a 
general method for finding time varying control laws for 
driftless systems. In Reference 6 and 7, the authors develop 
smooth, time varying and piecewise continuous control laws. 
While these controls employ closed loop feedback, none 
considers the optimality of the solution. In this research, 
we apply optimal control theory to the mobile robot problem. 

B. STATE AND COSTATE EQUATIONS FOR THE MOBILE ROBOT 

In our approach to the mobile robot control problem we 
first move the robot onto the line described by the final 
position and orientation of the robot. (Fig. 13) The robot 
may then roll directly to the final desired position. The 
point at which the robot will intersect the line and the 
manner in which the robot will approach the line is not 
specified. The goal of the optimal control problem is to 
minimize the distance between the desired position of the 
robot and the point of intersection of the robot with the 
line, in a way that utilizes the minimum amount of energy. 
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1. Basic Kinematic Relationships 

Returning to the coordinate and velocity descriptions 
of Figure 1, we begin with the kinematic equations, 

X = U^cobQ 
y=c;jsine 
6 = 

From the desired final conditions of X^, Y^, and 6^, we 

redefine our states in terms of the difference between the 
final condition and the current coordinate value such that 
Lx = X^- X 

LY ^ Y^- Y (48) 

Ae = - e 

As our approach suggests, we require that the 
difference between the robot angle and desired angle be 
minimized or, 

A8 = 0 (49) 

We also require that the perpendicular distance between the 
robot and the line be minimized. This distance can be defined 
in trigonometric terms as 

p s (Ay cos6;, - LX sin0,j) (50) 

In order to converge p and LB to zero asymptotically, 
we find that the second input should be a function of the 
first input. Our analysis is based on the application of 
Lyapunov's Stability Theorem. 
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2. Application o£ Lyapunov's Theorem 


Lyapunov's Theorem of asymptotic stability provides 

that the equilibrium of zero for a system, 

- (51) 

x=f{x, u) 

is asymptotically stable if there exists a positive definite 
function such that the first derivative of that function is 
non-increasing. [Ref. 9] In our case we define a Lyapunov 
function as 

V = |<p2 + Ae^) (52) 

The first derivative of this function is. 


where 


V = -p(cos0,j sin0 - sin0,j cos0) U^-AdU2 
= p[4sin(A0) - A0i7j 
= -A0(t4 - 

= -A0 (t/j - pU^f (AO) ) 

P(A0) ^ 


(53) 


(54) 


If we choose 

C /2 = pUj^ f(A6) + aA0 (55) 

We may express equation (53) as 

V = -aA02 (56) 

which is negative semidefinite. 
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This equation satisfies Lyapunov's Theorem for all a greater 
than zero, provided that A$ is not equal to zero. In the 
event AO is equal to zero, the derivative of the Lyapunov 
function becomes negative semidefinite and the asymptotic 
stability can be guaranteed by applying the theorem by 
Mukherjee and Chen [Ref. 10]. 

The choice of the second control, U2, given by 
equation (55) , in terms of the first control, £7^, and state 
feedback leaves us with the task for the design of one input 
for the system, namely Uj. will be designed using optimal 
control methods. The gain, a, affects the rotational motion, 
and from Lyapunov's Theorem, a must be greater than zero at 
all times. Various schemes have been tested to determine the 
best use of this parameter in eui optimal solution for £7^. 

3. Variations of the Robot Problem 

Since the only requirement of £7^ is that equation (53) 
be negative definite, there are infinite variations of this 
function which we could employ. In the sections below, we 
produce five possible variations and discuss the application 
of optimal control to each of them. 

a. Robot 1, Virtual Robot Problem 

In this approach, in addition to the original 
robot, we define a virtual robot which may travel only on the 
line of the desired angle. (Fig. 14) This approach is 
somewhat similar to the bi-directional approach. [Ref. 11] The 
virtual robot may roll forward or backward, but not turn. 
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Our goal is to have the two robots meet at some unspecified 


point on the line. This allows a smooth trajectory for each 
robot. Furthermore, this positions and orients the real robot 
in line with the desired final location, requiring only a 
trivial solution to complete. Fortunately, the impact of this 
addition to the kinematic equations is minimal. Defining the 
position of the virtual robot as and Y^, which are now 
variables, the difference between the two positions is 
AX - {X^-X) 

AY = (Fj-F) 

The difference between the orientation of the two robots is 
A0 = (6^ - 6) (58) 


where 9^ is the constant desired angle of orientation, 
differential equations of motion now take the form. 


|(AX)| 


COS0,^I7,J - cos(0^ - A0) 

(AF) 

' = 

sinO^C/^ - sin(0j - A0) 

1(A0)] 


-aA0 - (A0) 


Where is the forward/backward velocity of the virtual 
robot. Applying the same optimal control theory as before, we 
define our cost function as 


J-= I (AJf2 + AF^ + A0^) + 


(60) 


The terminal cost gives a penalty for not going to the desired 
final condition, AX, AY, and AO. C is the weighting parameter 
for this cost. This cost is necessary as the values of the 
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final states tend to float and hence we cannot assume as 
before that 

6i(tf)=0 (61) 

From equation (9) we see that to satisfy the necessary 
conditions for optimal control, we need 

at t=t, (62) 

dx 

where 

4) = |(AX2 + + AO^) (63) 

This implies that the constraints at the final times are 
= C(A^),^ 

(Ij) (.' = C’(Ay) (64) 

= c(Ae),' 

As we shall see later, this is important in minimizing the 
error at the final time. From the definition of the 
Hamiltonian, 

H=L*X'^f (65) 


where 

L=\(ul + aj) 


( 66 ) 


and f is the right hand side of equation (59) . We apply 
equation (12) for both Uj and U^. From this we can show that 
the optimal control inputs are. 


U^=X^cos(d^ - AO) + X^. 

and 


(e„ - AO) + X^pfiAQ) (67) 


( 68 ) 



Applying equations (10) and (11) we can develop a full set of 


equations for our two point boundary value problem. 


(AX) 


cosQ^U^ - cosOj - A0) 

sinQ^Uj - sin(0j - AS) 

(AY) 


-aA0 - pU^fiAB) 

(A0) 


-X^U^sinB^fiAd) 

A'i 


X^U^cosB^f(AB) 



[XiaiSin(0,j - Ae) 1 



- X 2 C 4 COS (0,J-A0) 



[ + X 3 (a + pU^f'iAB) ) \ 


where Uj^, and p, are as described above. The function 
f (A6) requires special handling due to the Ad term in the 
denominator. By L'Hopital's rule we know 


(70) 


Therefore, 

define 


and 


to maintain continuity during numeric processing we 


f{AQ) 


for Ae^ol 
1, for A0=o| 


(72) 


f'(A0) = 


cos(AO) 
A0 

0 , 


sin(A0) 

A02 


for AOs^o I 

for Ae=oJ 


(73) 


From equation (69) the utility of a numeric solution to the 
two point boundary value problem becomes clear. 


32 



The process for setting up the expressions 
for the computer program is similar to the rolling disk 
problem, although more lengthy and involved. The final 
expressions can be found in the DIFEQ.FOR subroutine of 
Appendix B. 

Upon testing the virtual robot problem, it became 
obvious that the discontinuous path was not a problem for the 
program. (Fig 15) A look at the velocity components explains 
why. (Fig 16) When broken into components the velocities are 
smooth. We also note that the non-zero velocities at the 
initial and final time are not a source of concern, as this is 
a kinematics problem. Furthermore, since this is a motion 
planning problem and not feedback control, a two robot model 
has no practical disadvantages. However, the next robot models 
we consider are based upon a single robot. 

b. Robot 2, Single Mobile Robot 

The kinematic equations of motion for this problem 
are similar to the equations for the two mobile robot except 
that and are fixed. As a result, 


and the kinematic equations appear as 
{AX) [ "Cos(e,, - AG) 

(aV) '= - ^6) ^1 

[-«A0 - pC7,f(Ae) 
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We define the cost function for this problem as 

J = I + 2^02) + J|(C7i) <76) 

which has the same terminal costs as the two robot problem. 
We again use the definition of the Hamiltonian 

H = L + <77) 

where 

i=|(uf) (78) 

and f is the right hand side of equation (75) . Applying 
equation (12) for Uj we can show that for optimal control, 
t/j=A.iCos(e<, - A0) + X2sin(ej - A0) + k^pf(Ad) (79) 

Applying equations (10) and (11) we again develop the 
equations for our two point boundary value problem. 




-cos (0,, - A0) 

(AX) 


-sin{0j - A0) 

(Ay) 


-aA0 - pU^f (A6) 

(AO) 


-X 3 t/^sinO^jf (A0) 

K 


XjUjCosOjf (A0) 

K 


[X3C4sin(0^ - A0) 1 

X, 


- Xj^TtCos (0J-A0) 



[ + X^(a * pU^f'iAB) ) ] 


which is the same as equation (69) except for the first two 
differential equations. The resulting expressions can be seen 
in the DIFEQ.FOR subroutine of Appendix C: 
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As expected, the problem works adequately for 
apparently non-smooth paths. (Fig. 17, 18) However, for the 
case where we ask the robot to change only its angle of 
orientation, the solution given indicates that the robot only 
spins without moving forward. Vfhile this solution is indeed 
optimal, it is evident from the definition of angular velocity 
in equation (80) that the robot turns without moving, 
c. Robot 3, Contraiaed Robot Model 

To contrain the angular rotation to prevent 
rotation when the robot is stopped, we must ensure that is 
entirely a function of In order to meet Lyapunov's 

Theorem we must ensure that equation (53) is negative at all 
times. In order to meet both requirements, we chose U 2 of the 
form 

U2 = pU^f(LQ) + ag(U^)A6 (81) 

where 

fl, for K #01 

gr(a,)=] ^ \ (82) 

|o, for 14 #oj 

This expression guarantees that Lyapunov's Theorem is 
satisfied. Application of equation (12) results in the 
control, 

Ui = liCOsOj - A0) + XjSin(ej - AO) + l,pf(A0) (83) 
which is the same control from Robot 2. 
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Applying the other necessary conditions for optimal control 
gives the equations for the two point boundary value problem. 




-cos (0^ - A0) Uj 

(AX) 


-sin (0^ - A0) 

(aV) 


-aAdgiU^) - pU^f(AQ) 

(A0) 


-Xj !7^sin0,jf (A0) 



X3 U^cosQ^fiAd) 

K 


\X^U^sin{d^ - A0) ] 



- X2C73Cos{0„-Ae) 



[ + X^iagiu^) + pqf'(A6) ) ] 


The resulting expressions, obtained as before, can be seen in 
the DIFEQ.FOR subroutine in Appendix D. 

The solutions for this new variation are, in most 
cases, the same as those from Robot 2. (Fig. 19) The most 
significant difference is for the case where we ask the robot 
to change only its angle of orientation. The solution is now 
a trivial one; the robot does not move. (Fig. 20) When AX and 
AY are zero, the quantity p is zero and thus, 


- Ae) + X^SiniS^ - Ad) 


(85) 


And if Xj^ 


and Xj 


are zero for all 
= 0 

giU^) = 0 


time, then 

( 86 ) 
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and therefore. 


= 0 

giu^) = 0 
Ajs: = 0 
Ay = 0 
Ae = 0 
x, = 0 

Xj = 0 
X3 = 0 

The terminal costs are met since 

( 88 ) 

where A6 is fixed and the X 3 term simply becomes a large 
enough constant to meet this constraint (Fig. 21} . 
Heuristically, this says that the most efficient manner to 
achieve the desired final condition is not to go. Such 
results occur any time two of the three states, AX, AY or A6, 
are equal to zero. In such cases where p or A6 is zero at all 
times and becomes zero, the state equations of motions from 
equation (84) are all equal zero and give a trivial solution. 

So far, we have chosen the value of cx at the 
outset of the program. However, as we shall show later, a has 


a direct impact 


the final solut: 



d. Robot: 4, Robot 3 with High/Low a Control 


Defining a as a control is complicated by the fact 
that a must be positive to satisfy Lyapunov's Theorem. We 
therefore define the cost function 

J = J (jUi + a) (89) 


where a is greater than zero for all t. The resulting 
Hamiltonian is 

H = + a 

-AjCOS<6j - A0) (90) 

-k^siniQ^ - A0) 

-X^{pU^f{A6) +ag(U^)Ad) 

Applying equation (12) to we find the same result as 
before, 

C/j = k^cosie^ - A0) + XjSinCOj - A0) + X,pf(A0) (91) 

For the second control we consider only those terms in the 
Hamiltonian associated with a. 

i4=a(l - AjSrCDi) A0) (92) 


In order to minimize the Hamiltonian and maintain a positive 
a we define 


where 




foi P > o 1 
for P s Oj 


(93) 


P = 1 - k^giUi) A0 (94) 

The resulting equations for the two point boundary value 
problem are the same as equation (84), except that the value 
of a depends on p. The resulting S expressions are listed in 
the DIFEQ.FOR program in Appendix E. 
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Using this variation of U2, the program has 
difficulty converging in many cases. The non-linear nature of 
a is the source of this difficulty. (Fig. 22) In many cases, 
the converged solution is the same as Robot 3. Since there 
appears to be little advantage to this variation, we seek a 
more proportional a control. 

e. Robot 5, Robot 3 with Proportional a Control 

To develop a proportional a control, we start with 
a new cost function, 

j = + a^) (95) 

We find that is the same as before. If we only consider 
the a terms then, 

2H^ = H = - 2X^agiUj^) AQ (96) 

However, since g(Uj^) equals zero for equal to zero, is 
already a minimum when equals zero and a approaches the 
positive side of zero. Thus we only need consider the case 
where is not zero. In this case g(U^) is one and will be 
dropped in the remaining expressions. Factoring we find, 

4 = (a - X,Ae)^ - iX^AO)^ (97) 

If we neglect the second part of this expression as it is not 
a function of a, then to minimize 

X 3 A 8 , for X 3 A 0 >O 
for XjAOsO 

where is some value greater than zero. 


(98) 


39 



The resulting differential equations differ from 
equation (84) only in that alpha is now a function of X 3 and 
A6. This must be taken into account when developing the S 
expressions. The changes to the resulting S expressions can 
be seen in the DIFEQ.FOR subroutine in Appendix F. 

In general, this variation gives better solutions 
than all other variations discussed. The proportional alpha 
control is more likely to converge and gives an apparently 
more optimal solution. It's tendency to converge is more well 
behaved than other variations. However, it still requires a 
certain amount of user interaction to set the value of amin 
and other program parameters to a value which will achieve 
convergence. Trends and comparisons of this and other 
variations of the mobile robot problem is the subject of the 
following section. 

C. DISCUSSION OF RESULTS 

There are many factors which affect convergence, 
optimality and error of the final solution. Since each 
variation was designed with a slightly different intent, 
comparison is difficult. This section discusses general 
trends noted during extensive testing of the programs. 

The state variable solutions to the two point boundary 
value problems are in terms of the difference between the 
current and desired value. For presentation, we convert these 
values into X, Y, and 0 coordinates. 
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In the case of Robot 1 desired coordinates move. 


Therefore, we substitute our solution back into the 
differential equations to obtain velocity profiles. For Robot 
1 only, we then use a crude trapezoidal integration to 
determine the values of X, Y, 9, and at each time. 
There is a small error imposed by this integration which may 
appear in the path plots for Robot 1. The true final error 
all cases is taken directly from the solution values of 6X, 
XY, and A9 and is not affected by this integration. By true 
final error we refer to the difference between the final robot 
coordinates and the desired robot coordinates. For most cases 
this true final error can be made negligibly small by 
adjusting a and C. 

For the other Robot variations the velocity profiles are 
obtained in the same way. However, since X^ and Y^ are fixed 
for these problems, determining the X, Y, and 0 coordinates is 
simply a matter of subtraction. The final error shown in 
these problems is more representative of the true final error. 

For the sake of comparison, the results of each variation 
was tested against a single pseudo-energy cost function. 

j = dt (99) 

Co 

This cost value has mixed units. For simplicty we consider 
the costs shown in the figures below in nondimensional units. 

Length^ {100) 

Time 
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While Robot 5 was the only variation developed from this 
performance parameter, this is the most encompassing cost 
description. The terminal costs were not considered for this 
part as these are compared in the form of final error. 

1. Effect of Varying a and C Parameters 

For each variation of the Robot program, the effect of 
varying the rotational gain, a, and terminal cost weighting, 
C, is different. Rather than present all the possible 
variations here, some of the more significant trends are 
sampled. 

The variation of a, strongly affects the ability of 
the program to converge as well as the optimality and error in 
the final solution. There is a range of a for which each 
program will converge for a given set of boundary conditions. 
A typical example of the effect of varying a can be seen in 
Figure 23. These are the paths given by Robot 1 for boundary 
conditions of 

= 0 X, = 10 

Va = 0 = 10 ( 101 ) 

00 =0° df = 90° 

We must also look at the angular trajectory for these 
solutions. (Fig. 24) Note that for the extreme values of a, 
there is a larger error in the final solution. Also note that 
the paths are of different lengths, indicating that some a's 
give more optimal solutions than others. The energy cost plot 
(Fig. 25) shows the effect of a on this cost. 
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From Figures 23 and 25, an a value of 25 appears to give both 


a minimum final error and cost. However, the results for 
this program and set of boundary conditions cannot be used as 
a guideline for all programs or cases. 

In the particular case of Robot 4, the use of and 

must be handled carefully. If the two values are greatly 
different, the program will have difficulty converging. If 
the two values are too close, there is no advantage to using 
this program. 

Robot 5 tends to converge for a larger range of a 
values. In general, any a for which the other programs 
converge will usually work for Robot 5. However in many 
cases, Robot 5 allows a lower a, and a lower final energy 
cost. As a rule of thumb, the lowest for which the 

program converges gives the most optimal solution. 

The variation of C is more straightforward; the higher 
the value of C, the smaller the final error. (Figs. 26, 27) 
However, certain limits do apply to this guideline. If C is 
too high for a given set of boundary conditions, the program 
tends not to converge. There is also a price for this 
accuracy. (Fig. 28) In general a more accurate final 
solutions show a higher final cost. 
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2. Sample of Test Cases 


The Robot programs have been tested for many different 
boundary conditions and, subject to user supplied parameters, 
give optimal solutions. The following six cases are 
representative of the results. For each program variation we 
provide the best known control parameters for that program and 
set of boundary conditions. In this way we compare the best 
result for each. 

a. 90 Degree Turn Problem 

For this case the boundary conditions are: 

Xg = 0 Xf ^ 10 

Fo = 0 Ff = 10 (102) 

Oj, = 0“ = 90° 

Most of the programs give a similar result for this problem, 
except Robot 1. (Fig. 29, 30) This problem requires a 

relatively high a to achieve convergence. As a result, the 
programs which use a as a control. Robot 4 & 5, show no 
advantage. (Fig. 31, 32) Although the Robot 1 solution takes 
a longer path, by our definition of cost, its solution is more 
optimal. 

b. 30 Degree Angle Parking Problem 

For this case the boundary conditions are: 

Xg = 0 Xf = 0 

Yo = 0 Ff = 2 (103) 

Bo =0° Bf = 30° 

Here, the effect of a control is more evident. (Fig. 33, 34) 
In the case of Robot 4, the program has a more difficult time 
converging because of the non-linear a. As a result, its 
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solution is the least optimal. (Fig. 35) Robot 5, however, 
gives the best optimal solution. While Robot 5's final error 
is higher than some others, considering the order of magnitude 
of the final error, the difference is negligible. 

c. 270 Degree Turn Problem 

This case is inherently not optimal. The angular 
displacement required could be achieved more easily by going 
to -90° instead. However, these boundary conditions provide 
a more demanding test. 

= 0 = 0 

To = 0 Ff = 2 (104) 
00 = 0° Of = 27 0° 

The programs overcome the angular displacement problem by 
stopping and backing part way though the maneuver. (Pig. 36, 
37, 38) Based on the final error and energy cost, no program 
has any distinct advantage over the others for this maneuver. 
(Fig. 39) 

d. 180 Degree Turn On A Point 

For all Robot programs using partial feedback, if 
two of the three state variables, AX, AY or A0, have zero 
difference between their initial and final boundary 
conditions, the result will be the trivial, "don't go" 
solution. If, however, there is at least small difference 
between the initial and final boundary conditions for two of 
the three states, a non-trivial solution can be obtained. For 
this reason, we use a small difference between the initial and 
final X boundary conditions for this problem. 
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Xq = 0 Xf = -0.01 

Fq = 0 Fjp = 0 (105) 

00 =0® Of = 180° 

Robot 1 and 2, which do not include partial feedback, simply 
turn and go to the desired position. (Fig. 40) The remaining 
solutions are all similar. For all cases the energy cost is 
the same, with similar final error for the feedback problems. 
(Fig. 41) 

e. Parallel Parking Problem 

This case proved the most difficult of any for the 
program to solve. Again, we must avoid the boundary 
conditions where two of the three boundary conditions have 
zero difference. Only Robot 5 was able to produce a solution 
with reasonable minimiim error. (Fig. 42, 43, 44) This is 
because only Robot 5 supports proportional a control. The 
choice of a is critical to the resulting solution. Since a is 
a gain which affects the angular velocity, too high an Q!n,in 
results in highly non-linear solutions. This is true even in 
the case of Robot 5. (Fig 45, 46, 47) Nevertheless, a 

judicious choice of results in an optimal and low final 

error solution. 
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f. Trivial Straight Line Case 


For the trivial case where we ask the robot to 
move along a straight line from one position to another, the 
solution converges very quickly to the obvious solution. A 
few iterations are necessary to bring the costate variables to 
their proper values. These programs have no problem 
converging, unlike the disk problem, because there are no 
competing boundary conditions. 

3. Other Trends Noted 

The effect on of the initial guess cannot be 
overstated. For each case, the initial guess was based on the 
straight line path between the initial and final points. 
(Fig. 48) The costates were assumed to be some small, non¬ 
zero value. The Robot programs show strong tendency towards 
convergence even when given such a crude guess. 

In some cases, particularly highly oscillatory 
solutions about small values, the program will tend toward 
convergence but then hover at some error value, or oscillate 
between two small error values. In these cases the best 
response is to adjust the SLOWC parameter to slow the program 
convergence. This causes the program to make smaller 
corrections where it might be jumping, back and forth, over a 
solution. Doing this does not guarantee a solution, but it is 
helpful in some cases. 
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The Program uaes an EPSILON variable to determine the 
value of f(AB) when AO is nearly equal to zero. If AO is less 
than EPSILON, we consider it sufficiently close to zero to use 
the definition of f(A0) equal to one and f' (AO) equal to zero. 
From experimenting with the programs, any reasonably small 
value for this variable will give the same solution. This is 
true as AO rarely approaches zero within a solution, except at 
the final time. At the final time, the computer determines 
the values of the expressions based on the final boundary 
condition expressions. Since the function f(A0) does not 
appear in these expressions, the value of EPSILON has no 
effect. In the case where AO is less than EPSILON at some 
other time, the impact appears negligible for all reasonable 
values of EPSILON. 

The contrained robot problems use the control of 
equation (82) . The program uses the variable EPSIL0N2 to 
determine if the value of is sufficiently close to zero for 
g(U^) to be equal to zero. The value used for this EPSIL0N2 
has been found to make little difference in the solution as 
is rarely zero for any length of time. (Fig. 49) Where the 
value of CTj is less than EPSIL0N2 at some time, the solution 
profile tends to be non-smooth, making it difficult (though 
not impossible) for the program to converge. On the next 
iteration, the time location of the zero CTj may be moved. As 
a result, the program solution tends to place the exactly zero 
velocities between the discrete time points. 
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Hence, for reasonably sized values of EPSIL0N2, the solution 
is not affected significantly. However, for convergence sake, 
the value of EPSIL0N2 should be sufficiently small, 1 x 10"^ 
or less. 
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IV. SUMMARY AND RECOMMENDATIONS 


In this thesis, we have demonstrated a method for finding 
an optimal, open loop, time varying control for a nonholonomic 
system. In general this method employs Pontryagin's 
Minimization Principle to find the state and costate equations 
for an optimal control. Then a numerical relaxation technique 
is applied to the resulting two point boundary value problem. 
For the specific problem of a two wheeled mobile robot, we 
first develop a partial feedback law using Lyapunov’s Theorem, 
In doing so, we create a system which does not fall under 
Brockett's Theorem and thus has an equilibrium point solution. 
The method has been found to give optimal solutions for all 
cases of the mobile robot problem. However, in the case where 
two of the three state boundary conditions are exactly the 
same at the initial and final time, the optimal solution 
obtained is a trivial one. The optimality of the solution is 
subject to the definition of the cost function, the weight of 
the terminal cost, and choice of rotational gain, a. While 
closed loop controls of mobile robots are obtainable, they are 
not optimal in any sense. For an application where efficiency 
is important, the method demonstrated here would be 
advantageous. 
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The results obtained opens up a number of areas for 


further research into this and related problems: 

• Refining the angular feedback g(U^) such that the 
control is more proportional to would result in a more 
smooth control. 

• A more refined algorithm for creating the initial guess 
of states and costates would help ensure convergence and allow 
more freedom in choosing problem parameters C and a. The 
initial guesses could be based on known solutions to some 
commonly used boundary conditions. 

• The method described here requires that the final time 
be fixed. A more general solution to the free end time, or 
minimum time problem is desirable. 

• This method could be used for in line path planning of 
a mobile robot. By using the last solution as the new initial 
guess, the program could constantly update the path for the 
robot to follow. As the last solution would be a very good 
guess, the program would converge very quickly. 

• The mobile robot problem presented is a kinematics 
problem. A more realistic second order problem would consider 
kinetic optimization, mass moment of inertia, smooth velocity 
profiles and initial and final velocities at zero. 

As this list suggests, the possibilities for expanding 
this research are enormous. 
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Figure 






state Variable Trajectories: Circular Initial Guess 



Path of Disk on X-Y Plane 
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Costate Variable Trajectories: Circular Final Solution 
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Path of Disk on X-Y Plane: 10 Rolls, 5.5 Turns 
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Figure 8 

State Variable Trajectories: 10 Rolls, 5.5 Turns, Final Solutic 



Figure 9 


Time 





Lambda 1 through 4 



Path on X-Y Plane: Nearly Straight Line Path 



Figure 11 
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Figure 14 




X ,Y .Theta .Xd ,Yd Velocities 


Path on X-Y Plane: Robot 1 



State Variable Velocity Profiles 
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Position on X-Y Plane: Robot 2 



State Variable Trajectories: Turning on a Point 
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Path on X-Y Plane: Robot 3 






Costate Variable Trajectories: Robots, Trivial Solution 



Variation of Alpha With Time: Robot 4 
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Figure 22 
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Alpha Trajectory Comparison for Robot Programs, 90 Degree Turn 



















Path Comparison of Robot Programs, 270 Degree Turn 



Figure 36 X 

Angle Trajectory Comparison of Robot Programs, 270 Degree Turn 
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Angle Error (degrees) 





















Figure 40 (c) 
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Figure 41 (a) Figure 41 (b) 
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Alpha Minimum^ 1 


Alpha Minimum= 2 











Alpha Minimum= 1 


Alpha Minimum= 2 










Variation of U1 with Time: Robot 5, Highly Oscillatory Solution 
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Figure 49 (a) Time 


Discrete Time Velocities of U1: Robot 5, Highly Oscillatory Solution 
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Figure 49 (b) Time 
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APPENDIX A 


Program Files Specific to Disk 
GENDISK.FOR 
DIFEQ.FOR 
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APPENDIX B 


Program Files Specific to Robot 1 

ROBOTl.FOR 

DIFEQ.FOR (for Robot 1) 
XLATOR.FOR 
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APPENDIX C 


Program Files Specific to Robot 2 
R0B0T2.FOR 

DIFEQ.FOR (for Robot 2) 
XLAT0R2.FOR 


108 




109 
























Ill 







112 










117 






80 F0RMAT(2X,6F15.4) 

81 FORMAT(2X.4F20.10) 

RETURN 
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APPENDIX D 


Program Files Specific to Robot 3 
ROBOTS.FOR 

DIFEQ.FOR (for Robot 3) 
XLATORS.FOR 
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APPENDIX E 


Progr2mi Files Specific to Robot 4 
R0B0T4.FOR 

DIFEQ.FOR (for Robot 4) 
XLAT0R4.FOR 
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APPENDIX F 


Program Files Specific to Robot 5 

ROBOTS.FOR 

DIFEQ.FOR (for Robot 5) 
XLAT0R5.FOR 
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SCALV {1 )=ABS(DELXO/2) + . 01 
SCALV(2)«ABS(DELYO/2}+.01 
SCALV{3)rABS(2 *DELTHETO/M) + .01 
SCALV(4)«AaS(SLl)+.01 
SCALV{5)=ABS(SL2)+.01 
SCALV(6)=ABS(SL3)+.01 
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APPEraSIX G 


Program Siibroutines Used by All Programs 
SOLVDE.FOR 
PINVS.FOR 
RED.FOR 
BKSUB.FOR 
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